# -*- coding: utf-8 -*-
# @Time : 2022/6/28 20:40
# @Author : cheney
# @File : steeringDao.py
# @Software: PyCharm
# @Site: www.cheney.cc

"""
    舵机的底层操作
"""

from pi.dao.ultrasonic.Adafruit_PWM_Servo_Driver import PWM
import time


class steeringDao:
    pwm = PWM(0x40, debug=False)

    servoMin = 150  # Min pulse length out of 4096
    servoMax = 600  # Max pulse length out of 4096

    pwm.setPWMFreq(50)  # Set frequency to 60 Hz

    # pwm.setPWM(0,0,153)

    def _setServoPulse(self, channel, pulse):
        pulseLength = 1000000.0  # 1,000,000 us per second
        pulseLength /= 50.0  # 60 Hz
        print(pulseLength, "us per period")
        pulseLength /= 4096.0  # 12 bits of resolution
        print("%d us per bit" % pulseLength)
        pulse *= 1000.0
        pulse /= (pulseLength * 1.0)
        # pwmV=int(pluse)
        print("pluse: %f  " % pulse)
        self.pwm.setPWM(channel, 0, int(pulse))

    # Angle to PWM
    def write(self, servonum, x, sleep_time=0):
        y = x / 90.0 + 0.5
        y = max(y, 0.5)
        y = min(y, 2.5)
        self._setServoPulse(servonum, y)
        time.sleep(sleep_time)


if __name__ == '__main__':
    while True:
        steer_dao = steeringDao()
        steer_dao.write(0, 150)
        time.sleep(1)
        steer_dao.write(0, 120)
        time.sleep(1)
        steer_dao.write(0, 90)
        time.sleep(1)
